#define k_pitch 0.1
#define k_roll 0.1
#define k_yaw 0.01
#define k_height 0.01
int val;
//#define mot1 6
//#define mot2 5
//#define mot3 9
//#define mot4 3
float thrust;


void Distribute(){
  //thrust=64+y_height*k_height;
  val=analogRead(0);
  thrust=map(val,0,1023,0,1000);
  mot1_out=thrust*(+ y_pitch*k_pitch - y_roll*k_roll);// + yaw  VR
  mot2_out=thrust*(- y_pitch*k_pitch + y_roll*k_roll);// + yaw  HL
  mot3_out=thrust*(+ y_pitch*k_pitch + y_roll*k_roll);// - yaw  VL
  mot4_out=thrust*(- y_pitch*k_pitch - y_roll*k_roll);// - yaw  HR
  
//  analogWrite(mot1,map(mot1_out,0,60,0,255));
//  analogWrite(mot2,map(mot2_out,0,60,0,255));
//  analogWrite(mot3,map(mot3_out,0,60,0,255));
//  analogWrite(mot4,map(mot4_out,0,60,0,255));
  m1.writeMicroseconds(map(mot1_out,0,1000,1000,2000));
  m2.writeMicroseconds(map(mot2_out,0,1000,1000,2000));
  m3.writeMicroseconds(map(mot3_out,0,1000,1000,2000));
  m4.writeMicroseconds(map(mot4_out,0,1000,1000,2000));

}
